import numpy as np
from scipy.signal import butter, filtfilt, resample, detrend
from scipy.signal import firwin, lfilter


def dataproc(x, fs, fsr, flo, fhi):
    # 输入：
    # 信号、采样频率、低截止频率、高截止频率（x, fs, fsr, flo, fhi）
    # 输出：
    # 经过滤波之后的信号（xproc）
    
    # 去除信号的线性趋势
    x = detrend(x, type='constant')
    
    if flo == 0:
        # 低通滤波器
        Filt = firwin(1000, fhi, fs=fs, pass_zero='lowpass')
    else:
        # 带通滤波器
        Filt = firwin(1000, [flo, fhi], fs=fs, pass_zero='bandpass')
    
    xfil = lfilter(Filt, 1.0, x)
    del x
    
    p, q = rat(fsr / fs)
    
    xfil = xfil[500:]
    
    if fsr == fs:
        xproc = xfil
    else:
        xproc = resample(xfil, int(len(xfil) * p / q))
    
    return xproc

def rat(x):

    from fractions import Fraction
    frac = Fraction(x).limit_denominator()
    return frac.numerator, frac.denominator


from pyproj import CRS, Transformer
import numpy as np

def gps_to_local(lat, lon, lat_origin, lon_origin, lat_end, lon_end, epsg):
    """
    输入：
    - lat, lon: 车辆行驶位置的纬度、经度数组
    - lat_origin, lon_origin: 桥梁起点的纬度、经度
    - lat_end, lon_end: 桥梁终点的纬度、经度
    - epsg: 桥梁所在位置的 EPSG 代号
    输出：
    - local_x, local_y: 车辆所在位置的局部坐标数组
    """
    # 定义投影坐标系
    proj = CRS.from_epsg(epsg)
    transformer = Transformer.from_crs(CRS.from_epsg(4326), proj, always_xy=True)  # WGS84 转 EPSG

    # 计算桥梁起点和终点的投影坐标
    x_o, y_o = transformer.transform(lon_origin, lat_origin)  # 桥梁起点
    x_e, y_e = transformer.transform(lon_end, lat_end)        # 桥梁终点

    # 计算车辆位置的投影坐标
    x, y = transformer.transform(lon, lat)  # 支持数组输入

    # 计算桥梁的长度和方向角
    L = np.sqrt((x_e - x_o)**2 + (y_e - y_o)**2)  # 桥梁长度
    theta = np.arctan2((y_e-y_o), (x_e - x_o))            # 桥梁方向角

    # 计算局部坐标
    xt = x - x_o
    yt = y - y_o
    local_x = yt * np.sin(theta) + xt * np.cos(theta)
    local_y = yt * np.cos(theta) - xt * np.sin(theta)

    return local_x, local_y

def p_sum_r(L_b, c0, N_c, local_x, f_sst, P_lm):
    """
    输入：
    - L_b: 桥梁长度
    - c0: 重叠率
    - N_c: 桥梁分段数量
    - local_x: 局部坐标 x（数组）
    - f_sst: 插值后频率（数组）
    - P_lm: 单个样本插值后的小波矩阵（二维数组，形状为 (len(f_sst), len(local_x))）
    输出：
    - P_sm: 小波插值聚合结果（二维数组，形状为 (len(f_sst), N_c)）
    """
    # 计算每段的长度和步长
    c = L_b / (1 + (N_c - 1) * (1 - c0))
    s = c - c0 * c
    sm = c / 2 + np.arange(N_c) * s
    
    # 初始化 P_sm
    P_sm = np.zeros((len(f_sst), N_c))
    
    # 遍历每个区间，进行求和
    for i in range(N_c):
        # 找到局部坐标满足条件的索引
        Index_r = (local_x < sm[i] + c / 2) & (local_x > sm[i] - c / 2)
        
        # 将符合条件的 P_lm 列求和
        P_sm[:, i] = np.sum(P_lm[:, Index_r], axis=1)
    
    return P_sm


